#include "headfile.h"

int speed_sum = 0;

//-------------------------------------------------------------------------------------------------------------------
//  @brief      编码器计数，速度获取
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void SpeedGet(void)
{
	//脉冲读取
	rCurSpeed = -qtimer_quad_get(QTIMER_1,RIGHT_A);
//        rCurSpeed = ftm_quad_get(ftm1);//4号车
	lCurSpeed = qtimer_quad_get(QTIMER_1,LEFT_A );
	CurSpeed = (lCurSpeed + rCurSpeed) >> 1;
	qtimer_quad_clear(QTIMER_1,RIGHT_A );
    qtimer_quad_clear(QTIMER_1,LEFT_A);
	speed_sum += CurSpeed;
	//舵机误差获取
	SteerError = SteerDuty - INITDUTY;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      计算期望速度
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void ExpectSpeedGet()
{
	switch (speed_type)
	{
	case 0:
		spdExp = spdExp0;    //停车用
		//g_speedUpFlg = 0;
		break;
	case 1:
		spdExp = spdExp1;   //正常速度
		//g_speedUpFlg = 1;
		break;
	case 2:
		spdExp = spdExp2;   
		// g_speedUpFlg = 2;
		break;
	// case 3:
	// 	spdExp = spdExp3;
	// 	//g_speedUpFlg = 3;
	// 	break;
	// case 4:
	// 	spdExp = spdExp4;
	// 	//g_speedUpFlg = 4;
	// 	break;
	// case 5:
	// 	spdExp = spdExp5;
	// 	break;
    case 3:
        spdExp=spdExp6;//上位机用速度
        break;
	default:
		spdExp = spdExp0;
		// g_speedUpFlg = 0;
	}
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      左轮pwm输出计算
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void LeftPiCalc()
{
	float kdet = (float)KDet / 10000.0f;
	if (!protect_flag)//这个Protect_Flag是电磁里的东西
	{
		leftExpect = (1 - kdet * SteerError) * spdExp;
	}
	// if(g_go_back)
	// {
	// 	leftExpect = GO_BACK_SPEED;
	// }
	// else if(FD_WaitFlag)
	// {
	// 	leftExpect=4;
	// }
    // else if(Get_StopLine)
    // {
    //     leftExpect=0;
    // }

	leftDrv.error_1 = leftExpect - lCurSpeed;

	leftDrv.p = g_speed_p;
	leftDrv.i = g_speed_i;
	leftDrv.d = g_speed_d;

	////if (leftDrv.error_1 > 10 || leftDrv.error_1 < -10) leftDrv.i = 0;		//积分分离

	leftDrv.d_duty = (int)(0.5 + leftDrv.p * (leftDrv.error_1 - leftDrv.error_2) + leftDrv.i * leftDrv.error_1 + leftDrv.d * (leftDrv.error_1 - 2 * leftDrv.error_2 + leftDrv.error_3));
	leftSpeedOut += leftDrv.d_duty;

	// if (g_StateMaster > WaitingStop && speed_type == 0)			//停车急停
	// {
	// 	if (leftDrv.error_1 < -3) leftSpeedOut = SPEED_OUT_MIN;
	// }



	leftDrv.error_3 = leftDrv.error_2;
	leftDrv.error_2 = leftDrv.error_1;

	if (leftSpeedOut > SPEED_OUT_MAX)
		leftSpeedOut = SPEED_OUT_MAX;
	else if (leftSpeedOut < SPEED_OUT_MIN)
		leftSpeedOut = SPEED_OUT_MIN;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      右轮pwm输出计算
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void RightPiCalc()
{
	float kdet = (float)KDet / 10000.0f;
	if (!protect_flag)
	{
		rightExpect = (1 + kdet * SteerError) * spdExp;
	}
	// if(g_go_back)
	// {
	// 	rightExpect = GO_BACK_SPEED;
	// }
	// else if(FD_WaitFlag)
	// {
	// 	rightExpect=4;
	// }
    //     else if(Get_StopLine)
    //     {
    //             rightExpect=0;
    //     }

	rightDrv.error_1 = rightExpect - rCurSpeed;

	rightDrv.p = g_speed_p;
	rightDrv.i = g_speed_i;
	rightDrv.d = g_speed_d;

	// //if (rightDrv.error_1 > 10 || rightDrv.error_1 < -10) rightDrv.i = 0;		//积分分离

	rightDrv.d_duty = (int)(0.5 + rightDrv.p * (rightDrv.error_1 - rightDrv.error_2) + rightDrv.i * rightDrv.error_1 + rightDrv.d * (rightDrv.error_1 - 2 * rightDrv.error_2 + rightDrv.error_3));
	rightSpeedOut += rightDrv.d_duty;

	// if (g_StateMaster > WaitingStop && speed_type == 0)				//停车急停
	// {
	// 	if (rightDrv.error_1 < -3) rightSpeedOut = SPEED_OUT_MIN;
	// }
	



	rightDrv.error_3 = rightDrv.error_2;
	rightDrv.error_2 = rightDrv.error_1;

	if (rightSpeedOut > SPEED_OUT_MAX)
		rightSpeedOut = SPEED_OUT_MAX;
	else if (rightSpeedOut < SPEED_OUT_MIN)
		rightSpeedOut = SPEED_OUT_MIN;

}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      电机PWM输出
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void GiveDriveDuty(void)
{
	//右轮
	if (rightSpeedOut >= 0)
	{
		pwm_duty(RIGHT_GO,rightSpeedOut); //右正
		pwm_duty(RIGHT_BACK,0);
	}
	else
	{
		pwm_duty(RIGHT_GO,0);
		pwm_duty(RIGHT_BACK,-rightSpeedOut);
	}
	//左轮
	if (leftSpeedOut >= 0)
	{
		pwm_duty(LEFT_GO,leftSpeedOut);
		pwm_duty(LEFT_BACK,0);//左正
	}
	else
	{
		pwm_duty(LEFT_GO,0);//左反
		pwm_duty(LEFT_BACK,-leftSpeedOut);
	}
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      清除速度暂存变量
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void CleanTmp(void)
{
	leftSpeedOut = 0;
	rightSpeedOut = 0;
	leftDrv.error_1 = 0;
	leftDrv.error_2 = 0;
	leftDrv.error_3 = 0;
	leftDrv.d_duty = 0;
	rightDrv.error_1 = 0;
	rightDrv.error_2 = 0;
	rightDrv.error_3 = 0;
	rightDrv.d_duty = 0;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      速度控制主程序
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void SpeedControl(void)
{
    static int lock_count=0;
	if (1 == g_drive_flag)
	{
		SpeedGet();
		ExpectSpeedGet();
		LeftPiCalc();
		RightPiCalc();
		GiveDriveDuty();
        if(spdExp>500)
        {
            if (lCurSpeed < 2 || rCurSpeed < 2)
                lock_count++;
            else
                lock_count = 0;
            if(lock_count>500)
                g_drive_flag=0;
        }
		// last_speed = speed_type;
	}
	else if (0 == g_drive_flag)
	{
		SpeedGet();
		CleanTmp();
		pwm_duty(RIGHT_GO,0);
        pwm_duty(RIGHT_BACK,0);
        pwm_duty(LEFT_GO,0);
        pwm_duty(LEFT_BACK,0);
		//    DistantStop += curSpeed;
	}
}
